# 导入必要的ROS 2库
import rclpy
from rclpy.node import Node
from tf2_ros import TransformListener,Buffer
from tf_transformations import euler_from_quaternion

# 定义TF监听器类
# 该类继承自Node，用于监听和获取坐标变换信息
class TFListener(Node):
    
    def __init__(self,node_name):
        """
        构造函数，初始化TF监听器节点
        :param node_name: 节点名称
        """
        # 初始化父类Node
        super().__init__(node_name)
        # 创建TF缓冲区，用于存储坐标变换信息
        self.buffer = Buffer()
        # 创建TF监听器，监听坐标变换并存储到缓冲区
        self.listener = TransformListener(self.buffer,self)
        # 创建定时器，每秒调用一次TFCallBack函数
        # 用于定期获取并处理坐标变换信息
        self.timer = self.create_timer(1,self.TFCallBack)

    def TFCallBack(self):
        """
        定时器回调函数，用于获取并处理坐标变换信息
        1. 从TF缓冲区获取map到base_footprint的变换
        2. 将四元数转换为欧拉角
        3. 打印机器人位姿信息
        """
        try:
            # 获取从map到base_footprint的坐标变换
            # map是全局地图坐标系，base_footprint是机器人底盘坐标系
            # 这个变换表示机器人在地图中的位置和姿态
            # 包括x,y,z三个方向的平移和绕x,y,z轴的旋转
            tf = self.buffer.lookup_transform("map","base_footprint",rclpy.time.Time(seconds=0),rclpy.time.Duration(seconds=1))
            transform = tf.transform
            
            # 将四元数转换为欧拉角
            # 四元数用于表示三维旋转，欧拉角更直观
            # 返回的欧拉角包括roll, pitch, yaw三个角度
            rotation_euler = euler_from_quaternion([
                transform.rotation.x,transform.rotation.y,
                transform.rotation.z,transform.rotation.w])
                
            # 打印平移、旋转四元数和欧拉角信息
            # 平移信息包含x,y,z坐标
            # 旋转信息包含四元数x,y,z,w分量
            # 欧拉角包含roll, pitch, yaw角度
            self.get_logger().info(f"平移:{transform.translation},旋转四元数:{transform.rotation},欧拉角:{rotation_euler}")
            
        except Exception as e:
            # 如果获取变换失败，打印错误信息
            # 可能原因：TF树未建立、坐标系不存在、超时等
            self.get_logger().info(f"不能获取坐标变换,原因:{str(e)}")            
            
# 主函数
def main():
    """
    ROS 2节点主函数
    1. 初始化ROS 2系统
    2. 创建TFListener节点实例
    3. 保持节点运行，处理回调
    4. 关闭ROS 2系统
    """
    # 初始化ROS 2系统
    # 必须在创建任何节点之前调用
    rclpy.init()
    
    # 创建TFListener节点实例
    # 节点名称为"tf_node"
    tf_node = TFListener("tf_node")
    
    # 保持节点运行，处理回调
    # 该调用会阻塞，直到节点被关闭
    rclpy.spin(tf_node)
    
    # 关闭ROS 2系统
    # 在节点停止运行后调用
    rclpy.shutdown()
    
    return
